#!/usr/bin/env python
import rospy
from aikit_arm.srv import aikit_arm_srv
from std_msgs.msg import Int32
rospy.wait_for_service("aikit_arm")
arm = rospy.ServiceProxy("aikit_arm", aikit_arm_srv)
rospy.init_node("arm_speed",anonymous=True)
arm_s = 50
arm_sleep = 0

def cb_arm(msg):
    global arm_sleep
    if msg.data == 0:
        arm_sleep = 0
    else:
        arm_sleep = 1.0/float(msg.data)
def getspeed():
    rospy.Subscriber("arm_speed", Int32, cb_arm)
    rospy.Subscriber("arm_control", Int32, cb_arm)

def set_arm_speed():
    global arm_s
    if arm_sleep > 0:
        arm_s -= 1
    elif arm_sleep < 0:
        arm_s += 1


    if arm_s>100:
        arm_s=100
    if arm_s<0:
        arm_s=0

    arm(arm_s)
    rospy.sleep(abs(arm_sleep))

if __name__ == '__main__':
	try:
		getspeed()
		while not rospy.is_shutdown():
		    set_arm_speed()
		rospy.spin()
	except:
		pass
